﻿using AntMovement.Core;
using AntMovement.Core.Attributes;
using AntMovement.Core.Domain;
using AntMovement.Core.Entity;
using AntMovement.Core.Hardware;
using AntMovement.Core.IParameter;
using AntMovement.Core.Logic;
using AntMovement.Core.Logic.Base;
using AntMovement.Core.Model;
using AntMovement.Handle.FPCAutomaticPlateSetting.Model;
using AntMovement.Parameter;
using AntMovement.Parameter.Model;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Reflection;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace AntMovement.Handle.FPCAutomaticPlateSetting
{
    [FlowAttributes(nameof(Calibration), 6)]
    public class Calibration : TaskBase
    {
        #region 轴变量
        private AxisMotion _axisX = default, _axisY = default, _axisR1 = default, _axisR2 = default, _axisR3 = default;
        private AxisMotion _axisZ1 = default, _axisZ2 = default, _axisZ3 = default, _axis料盘升降Z轴 = default;

        private Axis _axisEntityY;
        private Axis _axisEntityR1;
        private Axis _axisEntityR2;
        private Axis _axisEntityR3;
        private Axis _axisEntityX;

        private Axis _axisEntityZ1;
        private Axis _axisEntityZ2;
        private Axis _axisEntityZ3;

        private Axis _axisEntity料盘升降Z轴;
        #endregion

        #region 上升沿
        private bool _oneKeyHome = false;
        private bool _homeAlarm = false, _homeRisingEdge = false, _search = false;
        #endregion

        #region 定时器
        private Ton _tonHome = new Ton();
        private Ton _tonPicture = new Ton();
        private Ton _tonDelay = new Ton();
        private Ton _tonAxis = new Ton();
        private Ton _ton1 = new Ton();
        private Ton _ton2 = new Ton();
        private Ton _ton3 = new Ton();
        private Ton _ton4 = new Ton();
        private Ton _ton5 = new Ton();
        private Ton _ton6 = new Ton();
        private Ton _ton9 = new Ton();
        private Ton _ton10 = new Ton();
        #endregion

        #region 点位
        private PointPositionInfoModel _标定点位;
        #endregion

        #region 输入点
        #endregion

        #region 输出点
        private ICoil _光源;
        #endregion
        #region 振动盘
        private ISingleControl _左右震动;
        private ISingleControl _前后震动;
        private ISingleControl _中心震动;
        #endregion

        #region 报警变量
        private bool _alarmCalibration = false;
        #endregion

        #region 私有变量
        private Task _task = default;
        private TPSocket _tpSocket = null;
        private bool _heartbeat = false;
        private bool _heartbeatStarted = false;
        private int _count = 0, _counter = 0;
        private PointRoundTripModel _tripModel;
        private string _fixedPointName = string.Empty;
        private long _fixedPointPulse = 0;
        /// <summary>
        /// 振动盘步数
        /// </summary>
        private int _stepVibratingDisc = 0;
        /// <summary>
        /// 识别NG累计
        /// </summary>
        private int _ccdNgCount = 0;
        private bool _orientation = false;
        #endregion
        #region 视觉交互
        /// <summary>
        /// 命令
        /// </summary>
        private bool _calibration = false;
        /// <summary>
        /// 坐标
        /// </summary>
        private float x_current, y_current, r_current;
        /// <summary>
        /// 标定位置
        /// </summary>
        private byte position = 1;
        private double _x, _y, _r;
        private long _pulseX, _pulseY, _pulseR;
        private int _stepVI = 0;
        private double _resultCalibration = 0;
        private long[] points = new long[5];
        private bool _negative = false;
        private bool _start = false;
        private bool _alarmCommunicationException = false;
        #endregion
        public Calibration(ITemporaryParameter temporary, IIOParameter iop, IAxisParameter axisp, IDialogHost dialogHost, ILog log)
            : base(temporary, iop, axisp, dialogHost, log)
        {
            _axisX = axisp.GetAxisMotion("X轴");
            _axisY = axisp.GetAxisMotion("Y轴");
            _axisR1 = axisp.GetAxisMotion("R轴1#");
            _axisR2 = axisp.GetAxisMotion("R轴2#");
            _axisR3 = axisp.GetAxisMotion("R轴3#");

            _log = log;
            _axisEntityX = axisp.GetAxisEntity("X轴");
            _axisEntityY = axisp.GetAxisEntity("Y轴");
            _axisEntityR1 = axisp.GetAxisEntity("R轴1#");
            _axisEntityR2 = axisp.GetAxisEntity("R轴2#");
            _axisEntityR3 = axisp.GetAxisEntity("R轴3#");

            _axisEntityZ1 = axisp.GetAxisEntity("Z轴1#");
            _axisEntityZ2 = axisp.GetAxisEntity("Z轴2#");
            _axisEntityZ3 = axisp.GetAxisEntity("Z轴3#");
            _标定点位 = axisp.GetPointPosition("标定点位");
            _temporary.Calibration = false;
            _temporary.HomeComplete = false;
            _axisZ1 = axisp.GetAxisMotion("Z轴1#");
            _axisZ2 = axisp.GetAxisMotion("Z轴2#");
            _axisZ3 = axisp.GetAxisMotion("Z轴3#");
            _axis料盘升降Z轴 = axisp.GetAxisMotion("料盘升降Z轴");
            _axisEntity料盘升降Z轴 = axisp.GetAxisEntity("料盘升降Z轴");
            _左右震动 = iop.GetSingleControl("左右震动", "电机");
            _前后震动 = iop.GetSingleControl("前后震动", "电机");
            _中心震动 = iop.GetSingleControl("中心震动", "电机");
            _stepVibratingDisc = 0;
            _光源 = iop.GetCoil("光源");
        }

        public override void EmergencyStop()
        {

        }
        private Stopwatch _stopwatch = new Stopwatch();
        public override void Handle()
        {
            if (_temporary.SystemStatus.Auto)
            {
                _光源.Execute(true);
            }
            if (_task == null)
            {
                _task = Task.Run(() =>
                {
                    _tpSocket = new TPSocket("192.168.39.108");
                    _log.Info("启动视觉交互服务...");
                    while (_temporary.Status_Device != Core.Enum.StatusDeviceType.板卡未连接)
                    {
                        try
                        {
                            //_heartbeat = _tpSocket.Heartbeat();
                            _heartbeat = true;
                            if (_heartbeat)
                            {
                                if (_calibration)
                                {
                                    _log.Info($"{_temporary.CalibrationIndex}#吸嘴 第[{position}]点 请求标定坐标 X-[{x_current}] Y-[{y_current}] R-[{r_current}]");
                                    var dataCalibration = _tpSocket.Calibration((byte)_temporary.CalibrationIndex, x_current, y_current, r_current, position);
                                    _log.Info($"{_temporary.CalibrationIndex}#吸嘴 视觉返回原始数据：{string.Join(',', dataCalibration)}");
                                    _log.Info($"{_temporary.CalibrationIndex}#吸嘴 第[{position}]点 标定结果：[{(dataCalibration[1] != 2 ? "成功" : "失败")}]");
                                    _resultCalibration = dataCalibration[1];
                                    if (_resultCalibration == 2)
                                    {
                                        _dialogHost.DialogShow($"{_temporary.CalibrationIndex}#吸嘴 第[{position}]点 视觉标定失败，请重新标定...[注意：多次标定失败的请联系视觉供应商]", Core.Enum.LogLevel.Error);
                                        _log.Error("交互", $"{_temporary.CalibrationIndex}#吸嘴 第[{position}]点 视觉标定失败，请重新标定...");
                                    }
                                    _x = dataCalibration[2];
                                    _y = dataCalibration[3];
                                    _r = dataCalibration[4];
                                    _pulseX = DistanceToPulse(_x, _axisEntityX);
                                    _pulseY = DistanceToPulse(_y, _axisEntityY);
                                    switch (_temporary.CalibrationIndex)
                                    {
                                        case 1:
                                            _pulseR = DistanceToPulse(_r, _axisEntityR1);
                                            break;
                                        case 2:
                                            _pulseR = DistanceToPulse(_r, _axisEntityR2);
                                            break;
                                        case 3:
                                            _pulseR = DistanceToPulse(_r, _axisEntityR3);
                                            break;
                                    }
                                    _log.Info($"{_temporary.CalibrationIndex}#吸嘴 第[{position}]点 标定结束 X-[{_x}] Y-[{_y}] R-[{_r}] PulseX-[{_pulseX}] PulseY-[{_pulseY}] PulseR-[{_pulseR}]");
                                    _calibration = false;
                                }
                                if (_orientation)
                                {
                                    _temporary.BackOrientation.Clear();
                                    _log.Info($"第[{position}]点 请求定位");
                                    try
                                    {
                                        var datas = _tpSocket.PGE();
                                        if (_ccdNgCount >= 5)
                                        {
                                            _ccdNgCount = 2;
                                            _temporary.RrsultOrientation = 0;
                                        }
                                        else
                                        {
                                            _temporary.RrsultOrientation = (int)datas[1];
                                        }
                                        _log.Debug($"返回坐标值：{string.Join('-', datas)}");
                                        if (datas[1] == 1 && datas[2] > 2)
                                        {
                                            Queue<OrientationModel> orientations = new Queue<OrientationModel>();
                                            for (int i = 3; i < (datas.Count - 1); i += 3)
                                            {
                                                orientations.Enqueue(new OrientationModel()
                                                {
                                                    Index = i / 3,
                                                    X = datas[i],
                                                    Y = datas[i + 1],
                                                    R = datas[i + 2],
                                                });
                                            }
                                            for (int i = 0; i < 3; i++)
                                            {
                                                for (int j = 0; j < datas[2]; j++)
                                                {
                                                    var model = orientations.Dequeue();
                                                    _temporary.BackOrientation.Add(new BackOrientationModel()
                                                    {
                                                        Name = $"{(i + 1)}#",
                                                        Index = j,
                                                        X = model.X,
                                                        Y = model.Y,
                                                        R = model.R,
                                                    });
                                                }
                                            }
                                            _ccdNgCount = 0;
                                        }
                                        else
                                        {
                                            if (_temporary.RrsultOrientation != 0)
                                            {
                                                _ccdNgCount++;
                                            }
                                        }
                                    }
                                    catch (Exception)
                                    {
                                        _ccdNgCount++;
                                        _temporary.RrsultOrientation = 2;
                                        _alarmCommunicationException = true;
                                        _dialogHost.DialogShow("获取视觉定位数据通信异常!!!", Core.Enum.LogLevel.Error, hostDialog: Core.Enum.PopUpType.Global);
                                    }
                                    _temporary.Orientation = false;
                                    _orientation = false;
                                }
                            }
                        }
                        catch (Exception ex)
                        {
                            _log.Error("异常", $"视觉交互异常：{ex}");
                        }
                        Thread.Sleep(10);
                    }
                    _log.Warning("关闭视觉交互服务...");
                    _tpSocket.Exit();
                });
            }
            #region 初始化下聚拢震动
            if (_stepVibratingDisc == 0 && _temporary.SystemStatus.ManualOperation)
            {
                _左右震动.Manual_Open = true;
                _左右震动.Manual_Close = false;
                if (_左右震动.IOStatus)
                {
                    _stepVibratingDisc += 10;
                }
            }
            _ton9.Time(1200, _stepVibratingDisc == 10 && _temporary.SystemStatus.ManualOperation);
            if (_stepVibratingDisc == 10 && _ton9.Out && _temporary.SystemStatus.ManualOperation)
            {
                _左右震动.Manual_Open = false;
                _左右震动.Manual_Close = true;
                _前后震动.Manual_Open = true;
                _前后震动.Manual_Close = false;
                if (_前后震动.IOStatus && !_左右震动.IOStatus)
                {
                    _stepVibratingDisc += 10;
                }
            }
            _ton10.Time(800, _stepVibratingDisc == 20 && _temporary.SystemStatus.ManualOperation);
            if (_stepVibratingDisc == 20 && _ton10.Out && _temporary.SystemStatus.ManualOperation)
            {
                _前后震动.Manual_Open = false;
                _前后震动.Manual_Close = true;
                if (!_前后震动.IOStatus)
                {
                    _stepVibratingDisc = 100;
                }
            }
            #endregion
            #region 振动盘
            #region 聚拢震动
            if (_stepVibratingDisc == 40 && _temporary.SystemStatus.Auto)
            {
                _左右震动.Auto_Open = true;
                _左右震动.Auto_Close = !_左右震动.Auto_Open;
                if (_左右震动.IOStatus)
                {
                    _stepVibratingDisc += 10;
                }
            }
            _ton1.Time(1200, _stepVibratingDisc == 50 && _temporary.SystemStatus.Auto);
            if (_stepVibratingDisc == 50 && _ton1.Out && _temporary.SystemStatus.Auto)
            {
                _左右震动.Auto_Open = false;
                _左右震动.Auto_Close = !_左右震动.Auto_Open;
                _前后震动.Auto_Open = true;
                _前后震动.Auto_Close = !_前后震动.Auto_Open;
                if (_前后震动.IOStatus && !_左右震动.IOStatus)
                { 
                    _stepVibratingDisc += 10;
                }
            }
            _ton2.Time(800, _stepVibratingDisc == 60 && _temporary.SystemStatus.Auto);
            if (_stepVibratingDisc == 60 && _ton2.Out && _temporary.SystemStatus.Auto)
            {
                _前后震动.Auto_Open = false;
                _前后震动.Auto_Close = !_前后震动.Auto_Open;
                if (!_前后震动.IOStatus)
                {
                    _stepVibratingDisc = 100;
                    _ccdNgCount++;
                }
            }
            #endregion
            #region 振散
            if (_stepVibratingDisc == 100 && _temporary.SystemStatus.Auto)
            {
                if (_temporary.Orientation)
                {
                    if (_ccdNgCount == 2)
                    {
                        _stepVibratingDisc = 40;
                    }
                    else
                    {
                        _stepVibratingDisc += 10;
                    }
                }
            }
            if (_stepVibratingDisc == 110 && _temporary.SystemStatus.Auto)
            {
                _左右震动.Auto_Open = true;
                _左右震动.Auto_Close = !_左右震动.Auto_Open;
                _前后震动.Auto_Open = true;
                _前后震动.Auto_Close = !_前后震动.Auto_Open;
                if (_前后震动.IOStatus && _左右震动.IOStatus)
                {
                    _stepVibratingDisc += 10;
                }
            }
            _ton3.Time(500, _stepVibratingDisc == 120 && _temporary.SystemStatus.Auto);
            if (_stepVibratingDisc == 120 && _ton3.Out && _temporary.SystemStatus.Auto)
            {
                _左右震动.Auto_Open = false;
                _左右震动.Auto_Close = !_左右震动.Auto_Open;
                _前后震动.Auto_Open = false;
                _前后震动.Auto_Close = !_前后震动.Auto_Open;
                if (!_前后震动.IOStatus && !_左右震动.IOStatus)
                {
                    _stepVibratingDisc += 10;
                }
            }
            _ton5.Time(800, _stepVibratingDisc == 130 && _temporary.SystemStatus.Auto);
            if (_stepVibratingDisc == 130 && _ton5.Out && _temporary.SystemStatus.Auto)
            {
                _stepVibratingDisc += 10;
            }
            if (_stepVibratingDisc == 140 && _temporary.SystemStatus.Auto)
            {
                _orientation = true;
                _stepVibratingDisc += 10;
            }
            if (_stepVibratingDisc == 150 && _temporary.SystemStatus.Auto)
            {
                if (!_orientation)
                {
                    _stepVibratingDisc = 100;
                }
            }
            #endregion
            #endregion

            if (_temporary.SystemStatus.ManualOperation)
            {
                if (_temporary.Calibration)
                {
                    if (!_homeAlarm && !_alarmCalibration)
                    {
                        if (_stepVI == 0)
                        {
                            switch (_temporary.CalibrationIndex)
                            {
                                case 1:
                                    _axisR1.Move(_标定点位.YPulse + _pulseR, 30000, difference: 0);
                                    if (_axisR1.ArrivalLocation)
                                    {
                                        _stepVI = 10;
                                    }
                                    break;
                                case 2:
                                    _axisR2.Move(_标定点位.YPulse + _pulseR, 30000, difference: 0);
                                    if (_axisR2.ArrivalLocation)
                                    {
                                        _stepVI = 10;
                                    }
                                    break;
                                case 3:
                                    _axisR3.Move(_标定点位.YPulse + _pulseR, 30000, difference: 0);
                                    if (_axisR3.ArrivalLocation)
                                    {
                                        _stepVI = 10;
                                    }
                                    break;
                            }
                        }
                        if (_stepVI == 10)
                        {
                            if (PointPositionMove(_标定点位.XPulse + _pulseX, _标定点位.YPulse + _pulseY))
                            {
                                x_current = PulseToDistance(_axisEntityX);
                                y_current = PulseToDistance(_axisEntityY);
                                switch (_temporary.CalibrationIndex)
                                {
                                    case 1:
                                        r_current = PulseToDistance(_axisEntityR1);
                                        break;
                                    case 2:
                                        r_current = PulseToDistance(_axisEntityR2);
                                        break;
                                    case 3:
                                        r_current = PulseToDistance(_axisEntityR3);
                                        break;
                                }
                                _stepVI = 20;
                            }
                        }
                        if (_stepVI == 20)
                        {
                            _stepVI += 10;
                        }
                        if (_stepVI == 30)
                        {
                            _tonPicture.Time(500, true);
                            if (_tonPicture.Out)
                            {
                                _calibration = true;
                                _stepVI += 10;
                                _tonPicture.Time(500, false);
                            }
                        }
                        if (_stepVI == 40)
                        {
                            if (!_calibration)
                            {
                                if (_resultCalibration == 2)
                                {
                                    _alarmCalibration = true;
                                }
                                else
                                {
                                    _stepVI += 10;
                                }
                            }
                        }
                        if (_stepVI == 50)
                        {
                            if (_resultCalibration == 3)
                            {
                                position++;
                                _stepVI = 0;
                            }
                            else
                            {
                                _stepVI = 0;
                                position = 1;
                                _temporary.Calibration = false;
                            }
                        }
                    }
                }
                #region 一键回原
                if (_temporary.OneKeyHome)
                {
                    _tonHome.Time(1000, true);
                    if (_tonHome.Out)
                    {
                        _oneKeyHome = true;
                        _temporary.OneKeyHome = false;
                    }
                }
                else
                {
                    _tonHome.Time(1000, false);
                }
                if (_oneKeyHome)
                {
                    if (!_axisZ1.HomeComplete)
                    {
                        _axisZ1.Home();
                    }
                    if (!_axisZ2.HomeComplete)
                    {
                        _axisZ2.Home();
                    }
                    if (!_axisZ3.HomeComplete)
                    {
                        _axisZ3.Home();
                    }
                    if (!_axis料盘升降Z轴.HomeComplete)
                    {
                        _axis料盘升降Z轴.Home();
                    }
                    if (_axisZ1.HomeComplete && _axisZ2.HomeComplete && _axisZ3.HomeComplete)
                    {
                        if (!_axisX.HomeComplete)
                            _axisX.Home();
                        if (!_axisY.HomeComplete)
                            _axisY.Home();
                        if (!_axisR1.HomeComplete)
                            _axisR1.Home();
                        if (!_axisR2.HomeComplete)
                            _axisR2.Home();
                        if (!_axisR3.HomeComplete)
                            _axisR3.Home();
                    }
                    if (_axisX.HomeComplete && _axisY.HomeComplete && _axis料盘升降Z轴.HomeComplete && _axisZ1.HomeComplete && _axisR1.HomeComplete && _axisZ2.HomeComplete && _axisR2.HomeComplete && _axisZ3.HomeComplete && _axisR3.HomeComplete)
                    {
                        _oneKeyHome = false;
                        _temporary.HomeComplete = true;
                        _dialogHost.DialogShow("一键回原完成,允许开机！！！", Core.Enum.LogLevel.Success);
                    }
                }

                if (_temporary.RunPointRound)
                {
                    if (!_search)
                    {
                        _log.Info($"[{_temporary.NamePointRound}] 开始执行往返运动 次数[{_tripModel.Count}]");
                        _start = true;
                        Axis axisData = _axisParameter.GetAxisEntity(_temporary.NamePointRound);
                        double pe = axisData.UnitConversion.WeeklyItinerary / axisData.UnitConversion.CyclePulse;
                        _tripModel = _temporary.PointRoundTrips.FirstOrDefault(x => x.Name == _temporary.NamePointRound);
                        points[0] = (long)(_tripModel.PointOne / pe);
                        points[1] = (long)(_tripModel.PointTwo / pe);
                        points[2] = (long)(_tripModel.PointThree / pe);
                        points[3] = (long)(_tripModel.PointFour / pe);
                        points[4] = (long)(_tripModel.PointFive / pe);
                        _search = true;
                    }
                    _tonDelay.Time(400, !_start);
                    if (_tonDelay.Out)
                    {
                        _stopwatch.Restart();
                        _start = true;
                    }
                    if (_start)
                    {
                        switch (_temporary.NamePointRound)
                        {
                            case "X轴":
                                _axisX.Move(points[_counter], 30000);
                                break;
                            case "Y轴":
                                _axisY.Move(points[_counter], 30000);
                                break;
                        }
                        if ((_axisX.ArrivalLocation && (_temporary.NamePointRound == "X轴"))
                            || (_axisY.ArrivalLocation && (_temporary.NamePointRound == "Y轴")))
                        {
                            if (_counter == 1)
                            {
                                _stopwatch.Stop();
                                _log.Info($"第[{_count}]次 耗时：{_stopwatch.Elapsed.TotalMilliseconds} ms");
                                _negative = true;
                                _search = false;
                            }
                            if (_counter == 0)
                            {
                                _count++;
                                _start = false;
                                _negative = false;
                                _search = false;
                            }
                            if (!_negative)
                            {
                                _counter++;
                            }
                            else
                            {
                                _counter--;
                            }
                        }
                    }
                    //if (_count == (_tripModel.Count + 1))
                    //{
                    //    _search = false;
                    //    _counter = 0;
                    //    _negative = false;
                    //    _count = 0;
                    //    _temporary.RunPointRound = false;
                    //    _log.Info($"[{_temporary.NamePointRound}] 往返运动结束");
                    //}
                }
                #endregion
            }
            _temporary.SystemStatus.AlarmStop |= _alarmCommunicationException;
        }

        public override void Resetting()
        {
            if (_alarmCalibration)
            {
                _stepVI = 0;
                _calibration = false;
                _temporary.Calibration = false;
                _alarmCalibration = false;
            }
            if (_alarmCommunicationException)
            {
                _alarmCommunicationException = false;
            }
        }

        public override void SafeStop()
        {

        }

        #region 坐标计算
        private float PulseToDistance(Axis axis) => (float)Math.Round(GetPE(axis) * (double)axis.Status.CurrentPostion, 6, MidpointRounding.AwayFromZero);
        private float PulseToDistance(long pulse, Axis axis) => (float)Math.Round(GetPE(axis) * (double)pulse, 6, MidpointRounding.AwayFromZero);

        private long DistanceToPulse(double distance, Axis axis) =>  (long)Math.Round(distance / GetPE(axis), 6, MidpointRounding.AwayFromZero);

        private double GetPE(Axis axis)
        {
            double pe = 0;
            if (axis.UnitConversion.EnableGearbox)
            {
                if (axis.UnitConversion.EnableRotationMode)
                {
                    //旋转模式

                }
                else
                {
                    //线性模式
                    pe = (axis.UnitConversion.WeeklyItinerary * axis.UnitConversion.MotorGearRatio) / (axis.UnitConversion.CyclePulse * axis.UnitConversion.WorkingGearRatio);
                }
            }
            else
            {
                pe = axis.UnitConversion.WeeklyItinerary / axis.UnitConversion.CyclePulse;
            }
            return pe;
        }

        private bool PointPositionMove(long xPulse, long yPulse)
        {
            _axisX.Move(xPulse, 60000);
            _axisY.Move(yPulse, 60000);
            return _axisX.ArrivalLocation && _axisY.ArrivalLocation;
        }
        #endregion
    }
}
